#include "rgb_power.h"
#include "ws_constant.h"
#include "ws_response.h"
#include "ws_server_c.h"
#include "uart.h"
#include "elog.h"
#include "elog_file.h"
#include "state_manager.h"

#include <sys/shm.h>
#include <unistd.h>

#include <unordered_map>
#include <thread>
#include <functional>
#include <mutex>
#include <cmath>
#include <fstream>

using namespace std;

#define UART_SEND_DELAY_100MS       (100 * 1000)

#define TEXT_SZ 100
#define SHM_KEY 5678
#define MAX_WAIT_TIME 60

const char PRESS_KEY_MSG[] = "press_key";
const char START_RESET_MSG[] = "start_reset";

std::mutex rgbUpdateMutex;

struct shared_use_st {
    int written; // 0: 可写，非0: 可读
    char text[TEXT_SZ];
};
//串口相关
static int uart_fd = -1;
static mutex uart_mutex;

//RGB相关
static RgbControl rgb_control;
static bool rgb_switch = 1;     // 默认开RGB指示灯
static RgbState rgb_state = RGB_ON;
static mutex rgb_power_mutex;

//电源管理相关
static int cur_ele = 20;
static int global_ele = 20;
static bool press_key = false;
static bool b_shutdown = false;
static int power_charge_state = 0x31;
static ChargeState charge_state = CHARGE_NONE;
static PowerIndState power_ind_state = POWER_IND_ON;
static bool press_key_reset = false;
static bool b_disable_reverse = false;

static StateManager* state_manager = StateManager::GetInstance();

RgbPower::RgbPower() {

}

RgbPower::~RgbPower() {

}

static unordered_map<int, int (RgbPower::*)(WsPacket& ws_packet)> rgb_power_func_table {
    { CMD_RGB_POWER_POWER_DOWN,             &RgbPower::powerDown },
    { CMD_RGB_POWER_POWERIND_ON,            &RgbPower::powerIndOn },
    { CMD_RGB_POWER_POWERIND_OFF,           &RgbPower::powerIndOff },
    { CMD_RGB_POWER_REBOOT,                 &RgbPower::reboot },
    { CMD_RGB_POWER_OPEN_RGB,               &RgbPower::openRgb },
    { CMD_RGB_POWER_CLOSE_RGB,              &RgbPower::closeRgb }
};

int RgbPower::handleMessage(WsPacket& ws_packet) {
    log_i("handle rgb_power message, cmd = %d", ws_packet.cmd());

    //在表中查找函数指针并调用
    auto it = rgb_power_func_table.find(ws_packet.cmd());
    if (it != rgb_power_func_table.end()) {
        (this->*(it->second))(ws_packet);
    }
    else {
        log_i("Function not found");
    }

    return 0;
    
}

int create_and_attach_shm(void **shm, int *shmid) {
    *shmid = shmget((key_t)SHM_KEY, sizeof(struct shared_use_st), 0666|IPC_CREAT);
    if (*shmid == -1) {
        log_e("shmget failed");
        return -1;
    }

    *shm = shmat(*shmid, NULL, 0);
    if (*shm == (void *)-1) {
        log_e("shmat failed");
        return -1;
    }

    log_i("Memory attached at %X", (int)*shm);

    return 0;
}

void shmwrite_press_key() {
    void *shm = NULL;
    struct shared_use_st *shared = NULL;
    int shmid;

    if (create_and_attach_shm(&shm, &shmid) != 0) {
        return;
    }

    shared = (struct shared_use_st*)shm;

    if (shared->written == 0) {
        return;
    } else {
        printf("bt shm wrote: %s", shared->text);
        shared->written = 0;
    }

    if (shared->written == 0) {
        strncpy(shared->text, "press_key", 100);
        log_i("rgb shm wrote: %s", shared->text);
        shared->written = 1;
        sleep(1);
    }

    if (shmdt(shm) == -1) {
        log_e("shmdt failed");
    }
}

void RgbPower::waitPressKeyReset() {
    press_key = false;

    std::thread wait_press_key_reset_t([]() {
        int time_count  = 0;
        
        while (!press_key) {
            usleep(500 * 1000);
            time_count += 1;
            log_i("waiting press key");
            if (time_count > 60) {
                log_i("time out");
                break;
            }
        }

        if (press_key) {
            system("rm /userdata/cfg/init_config.json");
            system("sync");    
            press_key_reset = true;
        }
    });

    wait_press_key_reset_t.detach();
}

bool RgbPower::getPressKeyReset() {
    return press_key_reset;
}

void RgbPower::parseUartData(char *buf, int len) {
    //获取剩余电量
    if ((*buf) == 0x3E && (*(buf + 1)) == 0x34 && len == 6) {
        int cmd = *(buf + 1);
        int ele = *(buf + 2);
        int check = *(buf + 3);
        if(cmd + ele == check) {
            log_i("parse ele = %d", ele);
            cur_ele = ele;
        }
    }

    //获取充电状态
    if ((*buf) == 0x3E && (*(buf + 1)) == 0x35 && len == 6) {
        int cmd = *(buf + 1);
        int charge_value = *(buf + 2);
        int check = *(buf + 3);
        if(cmd + charge_value == check) {
            log_i("parse charge state = %d", *(buf + 2));
            if (charge_value == 0x31) {
                setChargeState(CHARGE_NONE);
                wsCommonResWithInt(CMD_NOTIFY_CHARGE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, CHARGE_NONE, 0);
            } else if (charge_value == 0x32) {
                setChargeState(CHARGE_SLOW);
                wsCommonResWithInt(CMD_NOTIFY_CHARGE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, CHARGE_SLOW, 0);
            } else {
                setChargeState(CHARGE_QUICK);
                wsCommonResWithInt(CMD_NOTIFY_CHARGE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, CHARGE_QUICK, 0);
            }
        }
    }

    //按键
    if ((*buf) == 0x3E && (*(buf + 1)) == 0x40) {
        press_key = true;
        log_i("press key");
        shmwrite_press_key();
    }

    //reverse charging
    if ((*buf) == 0x3E && (*(buf + 4)) == 0x04) {
        b_disable_reverse = true;
        log_i("receive disable reverse result");
    }
}

int RgbPower::uartRecvData() {
    static char rcv_buf[128] = {0};
    static int len_l = 0;
    int len = 0; 

    while (1)  //循环读取数据
    {
        len = UART_Recv(uart_fd, rcv_buf + len_l, sizeof(rcv_buf) - len_l);   
        if(len > 0)    
        {   
            len_l += len;
            if(len_l >= sizeof(rcv_buf)) {
                log_i("receive data missing terminator"); 
                memset(rcv_buf, 0, sizeof(rcv_buf));
                len_l = 0;
                continue;
            }

            if(rcv_buf[len_l - 2] == 0x0d && rcv_buf[len_l - 1] == 0x0a) {
                // printf("rcv_buf length is %d", len_l);
                // printf("receive data:");
                // for(int i = 0;i < len_l;i++) {
                //     printf("%02x ", rcv_buf[i]);
                // }
                // printf("\n");
                parseUartData(rcv_buf, len_l);

                //判断是否进行关机
                if (rcv_buf[0] == 0x3E && rcv_buf[1] == 0xff && rcv_buf[2] == 0xff && rcv_buf[3] == 0x0D && rcv_buf[4] == 0x0A) {
                    rgb_control.cycle_break = 1;
                    // b_shutdown = true;
                    log_i("关机!");
                    wsResNotifyPowerOff(CMD_NOTIFY_POWER_OFF, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION);
                    system("sync");
                }
                
                memset(rcv_buf, 0, sizeof(rcv_buf));
                len_l = 0;
            }

           
        }    
        else {    
            // log_i("cannot receive data");    
        }      
    }
    return 0;
}

int RgbPower::checkEle() {

    while(1) {
        getEle();
        sleep(30);
        log_i("checkEle global ele = %d", global_ele);
        wsCommonResWithInt(CMD_NOTIFY_ELE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, global_ele, 0);
    }

    return 0;
}

int RgbPower::initRgbPower() {
    int ret;
    log_i("init rgb_power");
    uart_fd = UART_Open(uart_fd, (char *)"/dev/ttyS4");  //打开串口，返回文件描述符
    ret = UART_Init(uart_fd, 460800, 0, 8, 1, 'N');

    if (uart_fd != -1 || ret != -1) {
        log_i("open uart4 successfully!");

        //串口接收数据
        thread uart_recv_t(&RgbPower::uartRecvData, this);
        uart_recv_t.detach();
        usleep(100 * 1000);
        setRgbEffect(RGB_EFFECT_UNCONNECTED);
        usleep(100 * 1000);
        thread t_disableReverseCharging(&RgbPower::disableReverseCharging, this);
        t_disableReverseCharging.detach();

    } else {
        log_i("open uart failed!");
    }

    return ret;
}


int RgbPower::uartSendData(char *send_buf,int data_len) {
    int ret;
    lock_guard<mutex> lock(uart_mutex);

    if (uart_fd == -1) {
        return -CODE_RGB_POWER_UART_INIT_FAILED;
    }

    ret =  UART_Send(uart_fd, send_buf, data_len);
    if (ret < 0) {
        log_i("uart send data failed!");
        return ret;
    }

    return 0;
}

// 禁用反向充电功能
int RgbPower::disableReverseCharging() {
    int ret;
    char send_buf[8] = {0};
    char send_buf_read[7] = {0};

    for (int i = 0;i < 5;i++) {
        memset(send_buf, 0, sizeof(send_buf));
        send_buf[0] = 0x3E;
        send_buf[1] = 0x45;
        send_buf[2] = 0x32;
        send_buf[3] = 0x28;
        send_buf[4] = 0x04;
        send_buf[5] = 0xA3;
        send_buf[6] = 0x0D;
        send_buf[7] = 0x0A;

        log_i("Disable reverse charging function");
        ret = uartSendData(send_buf, sizeof(send_buf));
    }

    usleep(200 * 1000);

    for (int i = 0;i < 5 && !b_disable_reverse;i++) {
        memset(send_buf_read, 0, sizeof(send_buf_read));
        send_buf_read[0] = 0x3E;
        send_buf_read[1] = 0x45;
        send_buf_read[2] = 0x31;
        send_buf_read[3] = 0x28;
        send_buf_read[4] = 0x9E;
        send_buf_read[5] = 0x0D;
        send_buf_read[6] = 0x0A;

        log_i("read Disable reverse charging function");
        ret = uartSendData(send_buf_read, sizeof(send_buf_read));
        
        usleep(200 * 1000);
    }

    if (!b_disable_reverse) {
        for (int i = 0;i < 5;i++) {
            memset(send_buf, 0, sizeof(send_buf));
            send_buf[0] = 0x3E;
            send_buf[1] = 0x45;
            send_buf[2] = 0x32;
            send_buf[3] = 0x28;
            send_buf[4] = 0x04;
            send_buf[5] = 0xA3;
            send_buf[6] = 0x0D;
            send_buf[7] = 0x0A;

            log_i("Disable reverse charging function");
            ret = uartSendData(send_buf, sizeof(send_buf));
        }
    }

    b_disable_reverse = true;
    
    return ret;
}

int RgbPower::powerOn() {
    int ret;
    char send_buf[6] = {0};

    send_buf[0] = 0x3E;
    send_buf[1] = 0x32;
    send_buf[2] = 0x36;
    send_buf[3] = 0x68;
    send_buf[4] = 0x0D;
    send_buf[5] = 0x0A;

    log_i("power on");
    ret = uartSendData(send_buf, sizeof(send_buf));
    
    return ret;
}

int RgbPower::powerDown() {
    int ret;
    char send_buf[6] = {0};

    send_buf[0] = 0x3E;
    send_buf[1] = 0x32;
    send_buf[2] = 0x38;
    send_buf[3] = 0x6a;
    send_buf[4] = 0x0D;
    send_buf[5] = 0x0A;

    ret = uartSendData(send_buf, sizeof(send_buf));
    if (ret == 0)
        log_i("power down");

    return ret;
}

int RgbPower::powerDown(WsPacket& ws_packet) {
    int ret = powerDown();
    wsCommonResponse(CMD_RGB_POWER_POWER_DOWN, MODULE_RGB_POWER, CMD_TYPE_RESPONSE, ret);

    return ret;
}

int RgbPower::reboot() {
    system("reboot");

    return 0;
}

int RgbPower::reboot(WsPacket& ws_packet) {
    wsCommonResponse(CMD_RGB_POWER_REBOOT, MODULE_RGB_POWER, CMD_TYPE_RESPONSE, WS_OK);
    reboot();

    return 0;
}

int RgbPower::powerIndOn() {
    int ret;
    char send_buf[6] = {0};

    send_buf[0] = 0x3E;
    send_buf[1] = 0x33;
    send_buf[2] = 0x31;
    send_buf[3] = 0x64;
    send_buf[4] = 0x0D;
    send_buf[5] = 0x0A;

    
    ret = uartSendData(send_buf, sizeof(send_buf));
    if (ret == 0) {
        log_i("powerInd on");
        power_ind_state = POWER_IND_ON;
        wsResNotifyPowerIndState(CMD_NOTIFY_POWER_IND_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, power_ind_state);
    }
    
    return ret;
}

PowerIndState RgbPower::getPowerIndState() {
    return power_ind_state;
}

int RgbPower::powerIndOn(WsPacket& ws_packet) {
    int ret = powerIndOn();
    wsCommonResponse(CMD_RGB_POWER_POWERIND_ON, MODULE_RGB_POWER, CMD_TYPE_RESPONSE, ret);

    return ret;
}

int RgbPower::powerIndOff() {
    int ret;
    char send_buf[6] = {0};

    send_buf[0] = 0x3E;
    send_buf[1] = 0x33;
    send_buf[2] = 0x30;
    send_buf[3] = 0x63;
    send_buf[4] = 0x0D;
    send_buf[5] = 0x0A;

    ret = uartSendData(send_buf, sizeof(send_buf));
    if (ret == 0) {
        log_i("powerInd off");
        power_ind_state = POWER_IND_OFF;
        wsResNotifyPowerIndState(CMD_NOTIFY_POWER_IND_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, power_ind_state);
    }
    
    return ret;
}

int RgbPower::powerIndOff(WsPacket& ws_packet) {
    int ret = powerIndOff();
    wsCommonResponse(CMD_RGB_POWER_POWERIND_OFF, MODULE_RGB_POWER, CMD_TYPE_RESPONSE, ret);

    return ret;
}

// 获取电量
int RgbPower::getEle() {
    char send_buf[5] = {0};

    send_buf[0] = 0x3E;
    send_buf[1] = 0x34;
    send_buf[2] = 0x34;
    send_buf[3] = 0x0D;
    send_buf[4] = 0x0A;

    uartSendData(send_buf, sizeof(send_buf));
    usleep(1000 * 100);

    global_ele = cur_ele;

    return global_ele;
}

// 设置RGB灯效
int RgbPower::setEffect(RgbLampEffect mode) {
    rgb_control.cycle_break = 1;
    std::lock_guard<std::mutex> lock(rgbUpdateMutex);

    if (rgb_switch == 1) {
        log_i("setEffect to %d", mode);
        rgb_state = RGB_ON;

        switch (mode) {
            case RGB_EFFECT_WORKING_STATE:
                if (state_manager->astro_.getCaptureDarkState() != STATE_IDLE) {
                    rgbClear();
                    rgb_state = RGB_OFF;
                } else if (state_manager->astro_.getCaptureLightState() != STATE_IDLE) {
                    astroStacking();
                } else if (state_manager->astro_.calibrater_.getCalibraterState() != STATE_IDLE) {
                    int success_times = state_manager->astro_.calibrater_.getSuccessTimes();
                    if (success_times == 0)
                        astroCalibrationStep1();
                    else if (success_times == 1)
                        astroCalibrationStep2();
                    else if (success_times == 2)
                        astroCalibrationStep3();
                } else if (state_manager->astro_.gotoer_.getGotoerState() != STATE_IDLE) {
                    astroGoto();
                } else if (state_manager->astro_.gotoer_.getTrackerState() != STATE_IDLE) {
                    astroTracking();
                } else if (state_manager->cam_tele_.getTimelapseState() != CAM_TIMELAPSE_IDLE) {
                    tlPhotograph();
                } else if (state_manager->cam_tele_.getPanoramaState() != CAM_PANORAMA_IDLE) {
                    panorama();
                } else if (state_manager->tracker_.getTrackState() != STATE_IDLE) {
                    tracking();
                } else {
                    if (-1 == getWsConnectionState())
                        unconnected();
                    else
                        connected();
                }
                break;
            case RGB_EFFECT_UNCONNECTED:
                unconnected();
                break;
            case RGB_EFFECT_CONNECTED:
                connected();
                break;
            case RGB_EFFECT_TRACKING:
                tracking();
                break;
            case RGB_EFFECT_PANORAMA:
                panorama();
                break;
            case RGB_EFFECT_TL_PHOTOGRAPH:
                tlPhotograph();
                break;
            case RGB_EFFECT_SUCCESS:
                success();
                break;
            case RGB_EFFECT_FAIL:
                fail();
                break;
            case RGB_EFFECT_ASTRO_CALIBRATION_SETP1:
                astroCalibrationStep1();
                break;
            case RGB_EFFECT_ASTRO_CALIBRATION_SETP2:
                astroCalibrationStep2();
                break;
            case RGB_EFFECT_ASTRO_CALIBRATION_SETP3:
                astroCalibrationStep3();
                break;
            case RGB_EFFECT_ASTRO_GOTO:
                astroGoto();
                break;
            case RGB_EFFECT_ASTRO_TRACKING: 
                astroTracking();
                break;
            case RGB_EFFECT_ASTRO_STACKING:
                astroStacking();
                break;
            case RGB_EFFECT_ASTRO_SUCCESS:
                astroSuccess();
                break;
            case RGB_EFFECT_ASTRO_FAIL:
                astroFail();
                break;
            case RGB_EFFECT_CLOSE:
                rgbClear();
                rgb_state = RGB_OFF;
                break;
        }
    } else {
        rgbClear();
        rgb_state = RGB_OFF;
    }

    return 0;
}

// 用线程的方式跑灯效，不阻塞，不会影响websocket的回调函数
int RgbPower::setRgbEffect(RgbLampEffect mode) {
    thread t_update(&RgbPower::setEffect, this, mode);
    t_update.detach();
    
    return 0;
}

int RgbPower::openRgb() {
    log_i("open rgb_control");
    setRgbState(RGB_ON);
    rgb_switch = 1;
    rgb_control.cycle_break = 1;
    setRgbEffect(RGB_EFFECT_WORKING_STATE);
    wsResNotifyRgbState(CMD_NOTIFY_RGB_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, rgb_state);

    return 0;
}

int RgbPower::openRgb(WsPacket& ws_packet) {
    int ret = openRgb();
    wsCommonResponse(CMD_RGB_POWER_OPEN_RGB, MODULE_RGB_POWER, CMD_TYPE_RESPONSE, ret);

    return ret;
}

int RgbPower::closeRgb() {
    log_i("close rgb_control");
    setRgbState(RGB_OFF);
    rgb_switch = 0;
    rgb_control.cycle_break = 1;
    rgbClear();
    wsResNotifyRgbState(CMD_NOTIFY_RGB_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, rgb_state);
    return 0;
}

int RgbPower::closeRgb(WsPacket& ws_packet) {
    int ret = closeRgb();
    wsCommonResponse(CMD_RGB_POWER_CLOSE_RGB, MODULE_RGB_POWER, CMD_TYPE_RESPONSE, ret);

    return ret;
}

int RgbPower::rgbControl(int mode, int hue, int saturation, int value, int background, int delay_time, int breathe_delay, int cycle_flag, int cycleNum) {
    int ret;
    char send_buf[16] = {0};

    send_buf[0] = 0x3E;
    send_buf[1] = 0x31;
    send_buf[2] = mode;
    send_buf[3] = hue;
    send_buf[4] = saturation;
    send_buf[5] = value;
    send_buf[6] = background;
    send_buf[7] = delay_time;
    send_buf[8] = breathe_delay;
    send_buf[9] = 0xFF;
    send_buf[10] = cycle_flag; // cycle_flag和cycleNum不能随便调换，虽然协议文档里说无效，但不符合的话灯效不生效
    send_buf[11] = cycleNum;
    send_buf[12] = 0xAF;

    //校验码
    for (int i = 1; i < 13; i++) send_buf[13] += send_buf[i];

    send_buf[13] = send_buf[13] & 0xFF;
    send_buf[14] = 0x0D;
    send_buf[15] = 0x0A;

    ret = uartSendData(send_buf, sizeof(send_buf));

    return ret;
}

int RgbPower::rgbClear() {
    log_i("RGB clear");
    rgbControl(0x41, 0x00, 0x00, 0x00, 0x00, 0x50, 0x05, 0x01, 0x03);
    usleep(20 * 1000);
    return 0;
}

// 未连接模式
int RgbPower::unconnected() {
    log_i("RGB unconnected");
    rgbClear();
    custom_mode_2(0x55, 0xff, 0xff, 0, 8, 1, 3);
    usleep(20 * 1000);
    return 0;
}

// 已连接模式
int RgbPower::connected() {
    log_i("RGB connected");
    rgbControl(0x11, 0x55, 0xff, 0xff, 0x00, 0x15, 0x03, 0x01, 0x03);
    usleep(20 * 1000);
    return 0;
}

int RgbPower::tracking() {
    log_i("RGB tracking");
    rgbControl(0x12, 0x55, 0xff, 0xff, 0x05, 0x15, 0x03, 0x01, 0x03);
    usleep(20 * 1000);
    return 0;
}

int RgbPower::panorama() {
    log_i("RGB panorama");
    rgbControl(0x13, 0x55, 0xff, 0xff, 0x05, 0x15, 0x03, 0x01, 0x03);
    usleep(20 * 1000);
    return 0;
}

int RgbPower::success() {
    log_i("RGB success");
    rgbControl(0x26, 0x55, 0xff, 0xff, 0x00, 0x15, 0x03, 0x01, 0x03);
    usleep(20 * 1000);
    return 0;
}
    
int RgbPower::fail() {
    log_i("RGB fail");
    rgbControl(0x11, 0xff, 0xff, 0xff, 0x00, 0x15, 0x03, 0x01, 0x03);
    usleep(20 * 1000);
    return 0;
}

int RgbPower::astroCalibrationStep1() {
    log_i("RGB astroCalibrationStep1");
    rgbControl(0x50, 0xff, 0xff, 0xff, 0x05, 0x15, 0x15, 0x01, 0x03);
    usleep(20 * 1000);
    return 0;
}

int RgbPower::astroCalibrationStep2() {
    log_i("RGB astroCalibrationStep2");
    rgbControl(0x51, 0xff, 0xff, 0xff, 0x05, 0x15, 0x15, 0x01, 0x03);
    usleep(20 * 1000);
    return 0;
}

int RgbPower::astroCalibrationStep3() {
    log_i("RGB astroCalibrationStep3");
    rgbControl(0x52, 0xff, 0xff, 0xff, 0x05, 0x15, 0x15, 0x01, 0x03);
    usleep(20 * 1000);
    return 0;
}


int RgbPower::astroGoto() {
    log_i("RGB astroGoto");
    rgbControl(0x53, 0xff, 0xff, 0xff, 0x05, 0x15, 0x15, 0x01, 0x03);
    usleep(20 * 1000);
    return 0;
}

int RgbPower::astroTracking() {
    log_i("RGB astroTracking");
    rgbClear();
    custom_mode_2(0xff, 0xff, 0xff, 0, 8, 1, 3);
    usleep(20 * 1000);
    return 0;
}

int RgbPower::astroStacking() {
    log_i("RGB astroStacking");
    rgbControl(0x25, 0xff, 0xff, 0xff, 0x00, 0x15, 0x20, 0x01, 0x03);
    usleep(20 * 1000);
    return 0;
}
    
int RgbPower::astroSuccess() {
    log_i("RGB astroSuccess");
    rgbControl(0x26, 0xff, 0xff, 0xff, 0x00, 0x15, 0x08, 0x01, 0x03);
    usleep(20 * 1000);
    return 0;
}

int RgbPower::astroFail() {
    log_i("RGB astroFail");
    rgbControl(0x27, 0xff, 0xff, 0xff, 0x00, 0x30, 0x08, 0x01, 0x03);
    usleep(20 * 1000);
    return 0;
}
    
int RgbPower::tlPhotograph() {
    log_i("RGB tlPhotograph");
    rgbClear();
    custom_mode_4(0xff, 0xff, 0xff, 0, 8, 1, 3);
    usleep(20 * 1000);
    return 0;
}

float RgbPower::min(float a, float b, float c) {
    float m;

    m = a < b ? a : b;
    return (m < c ? m : c);
}

float RgbPower::max(float a, float b, float c) {
    float m;

    m = a > b ? a : b;
    return (m > c ? m : c);
}

void RgbPower::hsv2rgb(uint8_t h, uint8_t s, uint8_t v, uint8_t *G, uint8_t *R, uint8_t *B) {
    float H = h * 1.411765;
    float S = s * 0.392157 / 100;
    float V = v * 0.392157 / 100;
    float C = S * V;
    float X = C * (1 - fabs(fmod(H / 60.0, 2) - 1));
    float m = V - C;
    float r, g, b;
    if (H >= 0 && H < 60) {
        r = C, g = X, b = 0;
    } else if (H >= 60 && H < 120) {
        r = X, g = C, b = 0;
    } else if (H >= 120 && H < 180) {
        r = 0, g = C, b = X;
    } else if (H >= 180 && H < 240) {
        r = 0, g = X, b = C;
    } else if (H >= 240 && H < 300) {
        r = X, g = 0, b = C;
    } else {
        r = C, g = 0, b = X;
    }
    *R = (r + m) * 255;
    *G = (g + m) * 255;
    *B = (b + m) * 255;
}

uint8_t RgbPower::gaussian2(uint32_t count, uint8_t value, uint8_t flag, int count_num) {
    double x;
    if (flag == 1)
        x = (5.0 / count_num) * count;  // 200次 由暗变到最亮
    else if (flag == 2)
        x = (5.0 / count_num) * (200 + count);  // 200次 由最亮变到暗
    else if (flag == 3) {
        if (count <= count_num / 2)
            x = (10.0 / count_num) * count;  // 160次 由暗变到最亮再变到暗
        else
            x = (10.0 / count_num) * (count_num / 2);  // 160次 由暗变到最亮再变到暗
    } else
        x = (10.0 / count_num) * count;  // 160次 由暗变到最亮再变到暗
    return (value * exp(-(pow(x - 5, 2.0) / (2.0 * 2))));
}

void RgbPower::gradual_count(unsigned char *value, int num) {
    static bool dir[8] = {false};
    if (*value < 255 && dir[num] == false)
        *value = *value + 1;
    else if (*value == 255 && dir[num] == false)
        dir[num] = true;
    else if (*value > 0 && dir[num] == true)
        *value = *value - 1;
    else if (*value == 0 && dir[num] == true)
        dir[num] = false;
}

int RgbPower::sendCustomData(uint8_t *buf) {
    int ret;
    
    static uint8_t send_buf[30] = {0};
    send_buf[0] = 0x3E;
    send_buf[1] = 0x39;
    memcpy(&send_buf[2], buf, 24);
    send_buf[26] = 0xFA;
    send_buf[27] = 0x00;
    //校验码
    for (int i = 1; i < 27; i++) send_buf[27] += send_buf[i];
    send_buf[27] = send_buf[27] & 0xFF;
    send_buf[28] = 0x0D;
    send_buf[29] = 0x0A;
    ret = uartSendData((char *)send_buf, sizeof(send_buf));
    
    return ret;
}

void RgbPower::gradual4(uint8_t *buf, int count_num) {
    if (rgb_control.fun_one_init == false) {
        rgb_control.fun_one_init = true;
        for (int i = 0; i < 8; i++) {
            rgb_control.gradual_count[i] = 0;
            rgb_control.count_flag[i] = false;
        }
        rgb_control.count_flag[0] = true;
        log_i("gradual4\r");
    }
    if (buf[7] >= 85 || rgb_control.count_flag[0] == true) {
        buf[0] = gaussian2(rgb_control.gradual_count[0], 255, 0, count_num);
        rgb_control.gradual_count[0]++;
        if (rgb_control.gradual_count[0] > count_num) {
            rgb_control.count_flag[0] = false;
            rgb_control.gradual_count[0] = 0;
        } else
            rgb_control.count_flag[0] = true;
    }
    if (buf[0] >= 85 || rgb_control.count_flag[1] == true) {
        buf[1] = gaussian2(rgb_control.gradual_count[1], 255, 0, count_num);
        rgb_control.gradual_count[1]++;
        if (rgb_control.gradual_count[1] > count_num) {
            rgb_control.count_flag[1] = false;
            rgb_control.gradual_count[1] = 0;
        } else
            rgb_control.count_flag[1] = true;
    }
    if (buf[1] >= 85 || rgb_control.count_flag[2] == true) {
        buf[2] = gaussian2(rgb_control.gradual_count[2], 255, 0, count_num);
        rgb_control.gradual_count[2]++;
        if (rgb_control.gradual_count[2] > count_num) {
            rgb_control.count_flag[2] = false;
            rgb_control.gradual_count[2] = 0;
        } else
            rgb_control.count_flag[2] = true;
    }
    if (buf[2] >= 85 || rgb_control.count_flag[3] == true) {
        buf[3] = gaussian2(rgb_control.gradual_count[3], 255, 0, count_num);
        rgb_control.gradual_count[3]++;
        if (rgb_control.gradual_count[3] > count_num) {
            rgb_control.count_flag[3] = false;
            rgb_control.gradual_count[3] = 0;
        } else
            rgb_control.count_flag[3] = true;
    }
    if (buf[3] >= 85 || rgb_control.count_flag[4] == true) {
        buf[4] = gaussian2(rgb_control.gradual_count[4], 255, 0, count_num);
        rgb_control.gradual_count[4]++;
        if (rgb_control.gradual_count[4] > count_num) {
            rgb_control.count_flag[4] = false;
            rgb_control.gradual_count[4] = 0;
        } else
            rgb_control.count_flag[4] = true;
    }
    if (buf[4] >= 85 || rgb_control.count_flag[5] == true) {
        buf[5] = gaussian2(rgb_control.gradual_count[5], 255, 0, count_num);
        rgb_control.gradual_count[5]++;
        if (rgb_control.gradual_count[5] > count_num) {
            rgb_control.count_flag[5] = false;
            rgb_control.gradual_count[5] = 0;
        } else
            rgb_control.count_flag[5] = true;
    }
    if (buf[5] >= 85 || rgb_control.count_flag[6] == true) {
        buf[6] = gaussian2(rgb_control.gradual_count[6], 255, 0, count_num);
        rgb_control.gradual_count[6]++;
        if (rgb_control.gradual_count[6] > count_num) {
            rgb_control.count_flag[6] = false;
            rgb_control.gradual_count[6] = 0;
        } else
            rgb_control.count_flag[6] = true;
    }
    if (buf[6] >= 85 || rgb_control.count_flag[7] == true) {
        buf[7] = gaussian2(rgb_control.gradual_count[7], 255, 0, count_num);
        rgb_control.gradual_count[7]++;
        if (rgb_control.gradual_count[7] > count_num) {
            rgb_control.count_flag[7] = false;
            rgb_control.gradual_count[7] = 0;
        } else
            rgb_control.count_flag[7] = true;
    }
}

void RgbPower::gradual6(uint8_t *buf, int count_num)  //双向
{
    if (rgb_control.fun_one_init == false) {
        rgb_control.fun_one_init = true;
        for (int i = 0; i < 8; i++) {
            rgb_control.gradual_count[i] = 0;
            rgb_control.count_flag[i] = false;
        }
        rgb_control.count_flag[0] = true;
        rgb_control.count_flag[4] = true;
        log_i("gradual6\r");
    }
    if (buf[3] >= 85 || rgb_control.count_flag[0] == true) {
        buf[0] = gaussian2(rgb_control.gradual_count[0], 255, 0, count_num);
        rgb_control.gradual_count[0]++;
        if (rgb_control.gradual_count[0] > count_num) {
            rgb_control.count_flag[0] = false;
            rgb_control.gradual_count[0] = 0;
        } else
            rgb_control.count_flag[0] = true;
    }
    if (buf[0] >= 85 || rgb_control.count_flag[1] == true) {
        buf[1] = gaussian2(rgb_control.gradual_count[1], 255, 0, count_num);
        rgb_control.gradual_count[1]++;
        if (rgb_control.gradual_count[1] > count_num) {
            rgb_control.count_flag[1] = false;
            rgb_control.gradual_count[1] = 0;
        } else
            rgb_control.count_flag[1] = true;
    }
    if (buf[1] >= 85 || rgb_control.count_flag[2] == true) {
        buf[2] = gaussian2(rgb_control.gradual_count[2], 255, 0, count_num);
        rgb_control.gradual_count[2]++;
        if (rgb_control.gradual_count[2] > count_num) {
            rgb_control.count_flag[2] = false;
            rgb_control.gradual_count[2] = 0;
        } else
            rgb_control.count_flag[2] = true;
    }
    if (buf[2] >= 85 || rgb_control.count_flag[3] == true) {
        buf[3] = gaussian2(rgb_control.gradual_count[3], 255, 0, count_num);
        rgb_control.gradual_count[3]++;
        if (rgb_control.gradual_count[3] > count_num) {
            rgb_control.count_flag[3] = false;
            rgb_control.gradual_count[3] = 0;
        } else
            rgb_control.count_flag[3] = true;
    }
    if (buf[7] >= 85 || rgb_control.count_flag[4] == true) {
        buf[4] = gaussian2(rgb_control.gradual_count[4], 255, 0, count_num);
        rgb_control.gradual_count[4]++;
        if (rgb_control.gradual_count[4] > count_num) {
            rgb_control.count_flag[4] = false;
            rgb_control.gradual_count[4] = 0;
        } else
            rgb_control.count_flag[4] = true;
    }
    if (buf[4] >= 85 || rgb_control.count_flag[5] == true) {
        buf[5] = gaussian2(rgb_control.gradual_count[5], 255, 0, count_num);
        rgb_control.gradual_count[5]++;
        if (rgb_control.gradual_count[5] > count_num) {
            rgb_control.count_flag[5] = false;
            rgb_control.gradual_count[5] = 0;
        } else
            rgb_control.count_flag[5] = true;
    }
    if (buf[5] >= 85 || rgb_control.count_flag[6] == true) {
        buf[6] = gaussian2(rgb_control.gradual_count[6], 255, 0, count_num);
        rgb_control.gradual_count[6]++;
        if (rgb_control.gradual_count[6] > count_num) {
            rgb_control.count_flag[6] = false;
            rgb_control.gradual_count[6] = 0;
        } else
            rgb_control.count_flag[6] = true;
    }
    if (buf[6] >= 85 || rgb_control.count_flag[7] == true) {
        buf[7] = gaussian2(rgb_control.gradual_count[7], 255, 0, count_num);
        rgb_control.gradual_count[7]++;
        if (rgb_control.gradual_count[7] > count_num) {
            rgb_control.count_flag[7] = false;
            rgb_control.gradual_count[7] = 0;
        } else
            rgb_control.count_flag[7] = true;
    }
}

int RgbPower::custom_mode_2(uint8_t h, uint8_t s, uint8_t v, int check_hsv, int delay, bool cycle_flag, int cycle_num) {
    uint8_t temp[8] = {0};
    uint8_t buf[24] = {0};
    if (check_hsv < 0 || check_hsv > 2) {
        log_e("HSV mode selection error");
        return -1;
    }
    rgb_control.cycle_break = 0;
    log_i("custom_mode_2");
    if (cycle_flag) {
        while (cycle_flag) {
            if (rgb_control.cycle_break) {
                rgb_control.cycle_break = 0;
                log_i("rgb_control cycle break");
                break;
            }

            gradual4(temp, 201);
            for (int i = 0; i < 8; i++) {
                hsv2rgb(h, s, temp[i], &buf[3 * i], &buf[3 * i + 1], &buf[3 * i + 2]);
            }

            sendCustomData(buf);
            usleep(delay * 1000);
        }
    } else {
        while (cycle_num--) {
            if (rgb_control.cycle_break) {
                rgb_control.cycle_break = 0;
                break;
            }

            gradual4(temp, 201);
            for (int i = 0; i < 8; i++) {
                hsv2rgb(h, s, temp[i], &buf[3 * i], &buf[3 * i + 1], &buf[3 * i + 2]);
            }

            sendCustomData(buf);
            usleep(delay * 1000);
        }
    }

    return 0;
}

int RgbPower::custom_mode_4(uint8_t h, uint8_t s, uint8_t v, int check_hsv, int delay, bool cycle_flag, int cycle_num) {
    unsigned char temp[8] = {0};
    unsigned char buf[24] = {0};
    unsigned char color_num[8] = {0};
    int count_t = 0;
    if (check_hsv < 0 || check_hsv > 2) {
        log_e("HSV mode selection error");
        return -1;
    }
    rgb_control.cycle_break = 0;
    log_i("custom_mode_4");
    if (cycle_flag) {
        while (cycle_num) {
            if (rgb_control.cycle_break) {
                rgb_control.cycle_break = 0;
                log_i("rgb_control cycle break");
                break;
            }

            gradual6(temp, 201);
            count_t++;
            if (count_t >= 10) {
                count_t = 0;
                gradual_count(color_num, 0);
            }
            for (int i = 0; i < 8; i++) {
                hsv2rgb(color_num[0], s, temp[i], &buf[3 * i], &buf[3 * i + 1], &buf[3 * i + 2]);
            }

            sendCustomData(buf);
            usleep(delay * 1000);
        }
    } else {
        while (cycle_num--) {
            if (rgb_control.cycle_break) {
                rgb_control.cycle_break = 0;
                break;
            }

            gradual6(temp, 201);
            for (int i = 0; i < 8; i++) {
                hsv2rgb(color_num[0], 255, temp[i], &buf[3 * i], &buf[3 * i + 1], &buf[3 * i + 2]);
            }

            sendCustomData(buf);
            usleep(delay * 1000);
        }
    }

    return 0;
}

void RgbPower::setRgbState(RgbState state) {
    rgb_state = state;
}

RgbState RgbPower::getRgbState() {
    return rgb_state;
}

void RgbPower::setChargeState(ChargeState state) {
    log_i("set charge state = %d", state);
    charge_state = state;
}

ChargeState RgbPower::getChargeState() {
    return charge_state;
}
